//
// Created by lqk on 2022/1/27.
//

#include "CentralMaintainer.h"


void CentralMaintainer::Run(const RuningEnvironment &environment) {

    m_runningEnv=environment;

    PeriodicTaskManager taskManager;
    QuadrupedSpecification miniCheetah;
    miniCheetah.m_legSpec.m_abadLinkLength=0.062;
    miniCheetah.m_legSpec.m_hipLinkLength=0.209;
    miniCheetah.m_legSpec.m_kneeLinkLength=0.195;
    miniCheetah.m_bodyLength=0.38;
    miniCheetah.m_bodyWidth=0.098;

    // ! Gazebo simulation deprecated
//    GazeboMotorsDriver motorsDriver(&taskManager,0.002,"gazebo_motors_driver");
//    motorsDriver.Start();
    PyBulletMotorsDriver motorsDriver(&taskManager,0.0009,"pyBullet_motors_driver","MotorsTorquePB","MotorsInfoPB");
    motorsDriver.Start();
    PyBulletCheaterSensorDriver cheaterSensorDriver(&taskManager,0.0009,"cheater_sensor_driver","CheaterSensorInfoPB");
    cheaterSensorDriver.Start();
    HighFrequencyTasksWrapper tasksWrapper(&taskManager,0.002,"high_freq_tasks_wrapper",miniCheetah);
    tasksWrapper.Start();


    while(true){
        taskManager.PrintAllStatus();
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    }

}


